%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 基于标准LMS算法的自适应噪声消除
% 生成测试信号
fs   = 1000;                   % 采样率
t    = (0:fs-1)'/fs;           % 1 秒采样点
f0   = 50;                     % 干净信号为 50 Hz 正弦
d    = sin(2*pi*f0*t);         % 期望信号 d[n]
n    = 0.5*randn(size(t));     % 白噪声
x    = d + n;                  % 测量输入 x[n] = d[n] + noise
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LMS 自适应滤波器参数
M     = 32;                    % 滤波器阶数（权向量长度 M+1）
mu    = 0.01;                  % 步长（学习率）
w     = zeros(M+1,1);          % 初始权重向量
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 迭代更新——直接型自适应 FIR 结构
y     = zeros(size(x));        % 输出
e_lms = zeros(size(x));        % 误差（滤波后残余噪声）
for n = M+1:length(x)
    xn       = x(n:-1:n-M);    % 输入向量 [x[n], x[n-1],…, x[n-M]]'
    y(n)     = w.' * xn;       % 滤波器输出 y[n]
    e_lms(n) = d(n) - y(n);    % 误差 e[n] = d[n] - y[n]
    w        = w + mu * e_lms(n) * xn;  % 权重更新（LMS 迭代）
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 基于归一化 LMS（NLMS）算法的自适应噪声消除
M      = 32;                   % 滤波器阶数（权向量长度 M+1）
mu     = 0.5;                  % 步长因子（0<mu<2）
delta    = 1e-6;               % 防止除零的小常数
w_nlms = zeros(M+1,1);         % 初始权重
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for n = M+1:length(x)
    xn        = x(n:-1:n-M);       % 当前输入向量
    y_nlms(n) = w_nlms.' * xn;     % 滤波输出
    e_nlms(n) = d(n) - y_nlms(n);  % 瞬时误差
    norm_x2   = xn.'*xn + delta;   % 输入向量能量
    w_nlms    = w_nlms + (mu/ norm_x2) * e_nlms(n) * xn;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 可视化结果
figure(1);
subplot(3,1,1);
plot(t, d, 'k'); title('期望信号 d[n]'); ylim([-1.5,1.5]);
subplot(3,1,2);
plot(t, x, 'r'); title('带噪声的测量信号 x[n]'); ylim([-1.5,1.5]);
subplot(3,1,3);
plot(t, y, 'b'); title('LMS 滤波输出 y[n]'); ylim([-1.5,1.5]);
figure(2);
plot(t, e_lms); title('LMS 残余误差 e[n]'); ylim([-1.5,1.5]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(3);
subplot(3,1,1);
plot(t, d, 'k'); title('期望信号 d[n]'); ylim([-1.5,1.5]);
subplot(3,1,2);
plot(t, x, 'r'); title('带噪声的测量信号 x[n]'); ylim([-1.5,1.5]);
subplot(3,1,3);
plot(t, y_nlms, 'b'); title('NLMS 输出 y[n]'); ylim([-1.5,1.5]);
figure(4);
plot(t, e_nlms);
title('NLMS 残余误差 e[n]'); ylim([-1.5,1.5]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 均方误差 (MSE) 学习曲线
window = 50;  % 平滑窗口长度
mse_lms  = movmean(e_lms.^2, window);
mse_nlms = movmean(e_nlms.^2, window);
figure(5);
semilogy(t, mse_lms, 'r', t, mse_nlms, 'b');
legend('LMS MSE','NLMS MSE');
xlabel('时间 (s)'); ylabel('均方误差 (dB)');
title('LMS vs NLMS 学习曲线（MSE）');